/*
 * @Author: your name
 * @Date: 2021-05-02 12:27:07
 * @LastEditTime: 2021-05-06 19:04:48
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \gcxz\USER\init.c
 */
#include "config.h"

uint32 getSysTime();    // 获取系统时间

void initCoreResource();
void initData();
void initDevice();

void initAll(){
    initCoreResource();	// 初始化芯片资源，包括uart、tim等
    initData();			// 初始化自定义的一些模块数据，如chassis、flag等
    initDevice();		// 初始化外设，如舵机、OLED
}

void initCoreResource(){

	// 串口初始化，波特率 = 115200
	uart_init(uart2, 115200);	// uart2用在蓝牙通信，使用字节接收中断 + DMA发送
	initUart_DMA(UART2);		// 必须在enable_irq之前调用，否则会出现一直进入tx中断
	set_irq_priority(UART2_RX_TX_IRQn, 2);
	uart_rx_irq_en(uart2);
	
	uart_init(uart4, 115200);	// uart4用在与edgeBoard通信，使用字节接收 + 字节发送
	initUart_DMA(UART4);		// 必须在enable_irq之前调用，否则会出现一直进入tx中断
	set_irq_priority(UART4_RX_TX_IRQn, 2);
	uart_rx_irq_en(uart4);
	
	// 定时器初始化
	pit_init_ms(pit0, 1);		// pit0用在系统时钟计数、任务周期控制
	set_irq_priority(PIT0_IRQn,1);
	enable_irq(PIT0_IRQn);

    //编码器初始化，用在L1,R1
	ftm_quad_init(ftm1); 					// LF
	ftm_quad_init(ftm2);					// RF
	gpio_init(A19, GPI, 0);					// RB
	port_init(B23, IRQ_FALLING | PF | ALT1 | PULLUP );	// 下降沿触发 | 带无源滤波器 | GPIO | 上拉
	gpio_init(A17, GPI, 0);					// LB
	port_init(A6, IRQ_FALLING | PF | ALT1 | PULLUP );	

    // 开启A、B端口中断
	set_irq_priority(PORTA_IRQn, 1);		// 设置优先级
	set_irq_priority(PORTB_IRQn, 1);		// 设置优先级
	enable_irq(PORTA_IRQn);
	enable_irq(PORTB_IRQn);

	EnableInterrupts;
}

void initData(){
    myPIDTimer::getMicroTick_regist(getSysTime);
    chassis.init();
	chassis.setMotorPid(35.0, 250.0, 0, 4000, 9500);	        // out_max不应该达到pwm的满值
	
	// TODO: X,Y,Z pid param
	chassis.pose_pid[X].SetPIDParam(10, 0.0, 2, 0, 9500);      // 底盘位置环的pid参数设置
	chassis.pose_pid[Y].SetPIDParam(10, 0.0, 2, 0, 9500);
	chassis.pose_pid[Z].SetPIDParam(75.0, 0.0, 2, 0, 9500);
}

void initDevice(){
    Bee_Init();
    Key_Init();
    OLED_Init();
    Steer_Init();
    Light_Init();       // 红灯绿灯初始化
}

/**
* @description: 返回us。对寄存器不熟悉，暂时返回ms*1000
 * @param {*}
 * @return {*}
 */
uint32 getSysTime(){

    return sys_time_ms * 1000;
}



